#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/LinearMath/Quaternion.h>

void cb(sensor_msgs::Imu msg)
{
    tf2::Quaternion q(
        msg.orientation.x,
        msg.orientation.y,
        msg.orientation.z,
        msg.orientation.w);
    tf2::Matrix3x3 mat(q);

    double roll, pitch, yaw;
    mat.getRPY(roll, pitch, yaw);

    // 弧度转换为角度
    roll = roll * 180.0 / M_PI;
    pitch = pitch * 180.0 / M_PI;
    yaw = yaw * 180.0 / M_PI;
    ROS_INFO("roll: %.2f, pitch: %.2f, yaw: %.2f", roll, pitch, yaw);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "imu");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("/imu/data", 10, cb);

    while (ros::ok()) {
        ros::spinOnce();
    }

    return 0;
}